#include "trajectory_predictor.hpp"
#include "kalman_filter.hpp"
#include <iostream>
#include <fstream>
#include <string>
#include <sstream>
#include <cmath>
#include <numeric>

using namespace std;

int main() {
    // string file = "D:\\HiLog-3XB0124805000169-No filters-DEBUG-output the abs_x(Regex)-1736946653690.txt";
    string file = "D:\\input2.txt";
    string savepath = "D:/";

    ifstream input_file(file);
    if (!input_file.is_open()) {
        std::cout << "无法打开文件！" << std::endl;
        return 1;
    }

    vector<string> data_lines;
    string line;
    while (getline(input_file, line)) {
        data_lines.push_back(line);
    }

    int len = data_lines.size();
    vector<double> time(len), abs_x(len), abs_y(len);
    vector<double> dxArr(len), dyArr(len);
    for (int i = 0; i < len; ++i) {
        std::stringstream ss(data_lines[i]);
        string dummy1, dummy2, dummy3, dummy4, dummy5;
        //读取数据的abs_x
        double _x, _y;

        double dx, dy;
        ss >> dummy1 >>dummy2 >> _x >> dummy3 >> _y >> dummy4 >> dx >> dummy5 >> dy;

        //日志日期转化为毫秒
        int hours, minutes, seconds;
        double millisseconds;
        char discard;
        std::stringstream ms(dummy1);
        ms >> hours >> discard >> minutes >> discard >> seconds >> discard >> millisseconds;
        double total_ms = (hours * 3600 + minutes * 60 + seconds) * 1000 + millisseconds;

        time[i] = total_ms;
        abs_x[i] = _x;
        abs_y[i] = _y;

        dxArr[i] = dx;
        dyArr[i] = dy;
    }

    double first_time = time[0];
    double first_x = abs_x[0];
    double first_y = abs_y[0];
    for (int i = 0; i < len; ++i) {
        time[i] -= first_time;
        abs_x[i] -= first_x;
        abs_y[i] -= first_y;
    }

    vector<double> xArr(len);
    // 使用std::partial_sum计算部分和
    std::partial_sum(dxArr.begin(), dxArr.end(), xArr.begin());

    vector<double> yArr(len);
    // 使用std::partial_sum计算部分和
    std::partial_sum(dyArr.begin(), dyArr.end(), yArr.begin());
    double first_x_ = xArr[0];
    double first_y_ = yArr[0];
    for (int i = 0; i < len; ++i) {
        xArr[i] -= first_x_;
        yArr[i] -= first_y_;
    }



    // Define refresh frequencies to iterate over
    vector<int> frequencies = { 120, 60, 30 };

    for (int f : frequencies) {
        cout << f << " Hz" << endl;

        // Generate refreshed cursor position (baseline)
        double refresh_time_inc = std::round(1000.0 / f);
        int refresh_len = static_cast<int>(time.back() / refresh_time_inc) + 1;

        vector<double> refresh_time(refresh_len);
        for (int i = 0; i < refresh_len; ++i) {
            refresh_time[i] = i * refresh_time_inc;
        }
        vector<int> refresh_index(refresh_len);

        for (int i = 0; i < refresh_len; ++i) {
            refresh_time[i] = i * refresh_time_inc;
            refresh_index[i] = 0;
        }

        for (int i = 1; i < refresh_len; ++i) {
            for (int k = refresh_index[i - 1]; k < len; ++k) {
                if (time[k] > refresh_time[i]) {
                    refresh_index[i] = k - 1;
                    break;
                }
            }
        }

        vector<double> refresh_abs_x(refresh_len), refresh_abs_y(refresh_len);
        for (int i = 0; i < refresh_len; ++i) {
            refresh_abs_x[i] = abs_x[refresh_index[i]];
            refresh_abs_y[i] = abs_y[refresh_index[i]];
        }

        // Kalman filter initialization
        KalmanFilter kf(10, 0.01);
        vector<double> refresh_abs_x_ex = refresh_abs_x;
        vector<double> refresh_abs_y_ex = refresh_abs_y;

        int last_frame = 0;
        double t_last = 0;
        vector<double> x_gain(len);
        vector<double> y_gain(len);
        for (int n = 0; n < refresh_index.back() + 1; ++n) {
            vector<PredictedPoint> v_PredictedPoint;

            // cpp(abs_x[n], abs_y[n], time[n], kf, t_last, x_pred, y_pred, t_pred);
            vector<PredictedPoint> predPoint = TrajectoryPredictor::predict_trajectory(xArr[n], yArr[n], time[n], kf, t_last, v_PredictedPoint);
            if (predPoint.empty()) {
                cout << "预测点为空，搞鬼" << endl;
                continue;
            } else {
//                cout << "predx:"<< predPoint[n].x << "==" << "predy:" << predPoint[n].y << endl;
            }
            vector<double> x_pred(predPoint.size()), y_pred(predPoint.size()), t_pred(predPoint.size());
            t_last = time[n];

            for (int i = 0; i < predPoint.size(); ++i) {
                x_pred[i] = predPoint[i].x + x_gain[n];
                y_pred[i] = predPoint[i].y + y_gain[n];
                t_pred[i] = predPoint[i].t;
            }
            for (int m = 0; m < 25; ++m) {
                if (t_pred[m] < time[n + 1]) {
                    if (t_pred[m] > refresh_time[last_frame + 1] - 1) {
                        refresh_abs_x_ex[last_frame + 1] = x_pred[m];
                        refresh_abs_y_ex[last_frame + 1] = y_pred[m];
                        last_frame += 1;
                        if (last_frame + 1 >= refresh_time.size()) {
                            break;
                        }
                    }
                } else {
                    x_gain[n + 1] = x_pred[m];
                    y_gain[n + 1] = y_pred[m];
                    break;
                }
            }
        }

        // Save data
        ofstream out_file;
        out_file.open(savepath + "refresh_abs_x_" + to_string(f) + "_Hz_perfect_circle.txt");
        for (double val : refresh_abs_x) {
            out_file << val << endl;
        }
        out_file.close();

        out_file.open(savepath + "refresh_abs_y_" + to_string(f) + "_Hz_perfect_circle.txt");
        for (double val : refresh_abs_y) {
            out_file << val << endl;
        }
        out_file.close();

        out_file.open(savepath + "refresh_abs_x_ex_" + to_string(f) + "_Hz_perfect_circle.txt");
        for (double val : refresh_abs_x_ex) {
            out_file << val << endl;
        }
        out_file.close();

        out_file.open(savepath + "refresh_abs_y_ex_" + to_string(f) + "_Hz_perfect_circle.txt");
        for (double val : refresh_abs_y_ex) {
            out_file << val << endl;
        }
        out_file.close();

        out_file.open(savepath + "refresh_time_" + to_string(f) + "_Hz_perfect_circle.txt");
        for (double val : refresh_time) {
            out_file << val << endl;
        }
        out_file.close();
    }

    return 0;
}